#include "mex.h"
#include<stdio.h>
#include <math.h>
int e,s,t,st,N,S,T,Ninlimits,*ind;
double dx,dz,*x,*z;
double *mscalar,*mx,*mz;
double *grid,*weight;
double xmin,xmax,zmin,zmax,mxmin,mxmax,mzmin,mzmax;

int IN0(int s,int t) {
    return t*S+s;
}

void markertorhogrid(int e,double value,double *grid,double *weight) {
    /*//gives a weighted contribution from a value on a marker to the four sorrounding rho grid points. Division with weights must be performed later*/
    int s,t;
    int INNV,INNE,INSV,INSE;
    double dlx,dlz,dw;
    s=get_s(mx[e]); t=get_t(mz[e]);
    INNV=IN0(s,t); INNE=IN0(s+1,t); INSV=IN0(s,t+1); INSE=IN0(s+1,t+1);
    dlx=(mx[e]-x[s])/dx;
    dlz=(mz[e]-z[t])/dz;
    /* // NV */
    dw=(1-dlx)*(1-dlz); grid[INNV]+=value*dw; weight[INNV]+=dw;
    /* // NE */
    dw=dlx*(1-dlz); grid[INNE]+=value*dw; weight[INNE]+=dw;
    /* // SV */
    dw=(1-dlx)*dlz; grid[INSV]+=value*dw; weight[INSV]+=dw;
    /* // SE */
    dw=dlx*dlz; grid[INSE]+=value*dw; weight[INSE]+=dw;
}

int getnearestmarker(double xpos,double zpos) {
    int nearestmarker;
    double dist2;
    double mindist2=DBL_MAX;
    for (e=0;e<Ninlimits;e++) {
        dist2=(xpos-mx[ind[e]])*(xpos-mx[ind[e]])+(zpos-mz[ind[e]])*(zpos-mz[ind[e]]);
        if (dist2<mindist2) {mindist2=dist2; nearestmarker=ind[e]; }
    }
    return nearestmarker;
}

void interpolate() {
    int nearest=0;
    int padding=0;
    for (e=0;e<Ninlimits;e++)  markertorhogrid(ind[e],mscalar[ind[e]],grid,weight);
    for (s=0;s<S;s++) {
        for (t=0;t<T;t++) {
            st=IN0(s,t);
            if (weight[st]>0) grid[st]/=weight[st]; 
            else {
                if (x[s]>=mxmin && x[s]<=mxmax && z[t]>=mzmin && z[t]<=mzmax)  { grid[st]=mscalar[getnearestmarker(x[s],z[t])]; nearest++; }
                else {grid[st]=mxGetNaN(); padding++; }
                
            }
        }
    }
    mexPrintf("Nearest marker interpolation: %f percent. Padding: %f percent\n",(double)nearest/(double)(S*T)*100,(double)padding/(double)(S*T)*100);
}
                    

int get_s(double xpos) {
    return floor((xpos-xmin)/(xmax-xmin)*(double)(S-1));
}
int get_t(double zpos) {
    return floor((zpos-zmin)/(zmax-zmin)*(double)(T-1));
}

void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[]) {
    mscalar=mxGetPr(prhs[0]);
    mx=mxGetPr(prhs[1]);
    mz=mxGetPr(prhs[2]);
    xmin=*mxGetPr(prhs[3]);
    xmax=*mxGetPr(prhs[4]);
    zmin=*mxGetPr(prhs[5]);
    zmax=*mxGetPr(prhs[6]);
    S=*mxGetPr(prhs[7]);
    T=*mxGetPr(prhs[8]);

    
    N=mxGetNumberOfElements(prhs[0]);
    
    plhs[0]=mxCreateDoubleMatrix(S,T, mxREAL);
    plhs[1]=mxCreateDoubleMatrix(S,1, mxREAL);
    plhs[2]=mxCreateDoubleMatrix(T,1, mxREAL);

    
    dx=(xmax-xmin)/(double)(S-1); dz=(zmax-zmin)/(double)(T-1);
    

    x=mxGetPr(plhs[1]); z=mxGetPr(plhs[2]);

    x[0]=xmin; for (s=0;s<S-1;s++) x[s+1]=x[s]+dx;
    z[0]=zmin; for (t=0;t<T-1;t++) z[t+1]=z[t]+dz;
    
    weight=mxCalloc(S*T,sizeof(double)); grid=mxGetPr(plhs[0]);
    
    for (s=0;s<S;s++) {  for (t=0;t<T;t++) { st=IN0(s,t); grid[st]=0; weight[st]=0; } }
    
    ind=mxCalloc(N,sizeof(int));
    Ninlimits=0;
    mxmin=mxGetInf(); mxmax=-mxGetInf(); mzmin=mxGetInf(); mzmax=-mxGetInf();
    for (e=0;e<N;e++) {
        if (mx[e]>xmin && mx[e]<xmax && mz[e]>zmin && mz[e]<zmax) {
        ind[Ninlimits]=e;
        Ninlimits++;
        if (mx[e]<=mxmin) mxmin=mx[e];
        if (mx[e]>=mxmax) mxmax=mx[e];
        if (mz[e]<=mzmin) mzmin=mz[e];
        if (mz[e]>=mzmax) mzmax=mz[e];
        }
    }
    
    
    interpolate();
    mxFree(weight); mxFree(ind);
}
